#!/usr/bin/env python
# coding:utf-8
import socket
import threading
import time
import serial
import serial.tools.list_ports
import math
import matplotlib.pyplot as plt
import numpy as np
import threading
import queue
import cmath
import open3d as o3d
import keyboard
import traceback
import sys
# import constants a
# NextFlag=False

port = "COM4"  # 端口号，根据自己实际情况输入，可以在设备管理器查看
bps = 115200     # 串口波特率，根据自己实际情况输入
timeout = 5       # 超时时间,None：永远等待操作，0为立即返回请求结果，其他值为等待超时时间(单位为秒）
rxdata = ''    # 接收的数据
rx_buf=[]
# rx_buf_show=[]
rx_buf_fin=[]
M_PI = 3.1415926
Angle_Fin = []
Distance_Fin = []

SHOWREALFLAG=False
SHOWSTOPFLAG = False

LIDARINIT=0
LIDARWAIT=1
LIDARSTART=2
LIDARNEEDTOWORK=3
LIDARWORKING=4
LIDARDONE=5
LIDARSTOP=6

MOTORINIT=0
MOTORWAIT=1
MOTORSTART=2
MOTORNEEDTOWORK=3
MOTORWOKING=4
MOTORDONE=5


LIDARSTATE = MOTORINIT
MOTORSTATE = MOTORINIT

COLLECTTIMES=3

YawAngle = 0
YAWANGLEMAX=360
LidarMeasureTimes=0
AngleLimit=180

def check_uart_port():
    port_list = list(serial.tools.list_ports.comports())
    # print(port_list)
    if len(port_list) == 0:
        print('can not fine uart port')
        return False
    else:
        for i in range(0,len(port_list)):
            print(port_list[i])
    return True

# 打开串口
def open_uart(port, bps, timeout):
    try:
        # 打开串口，并返回串口对象
        uart = serial.Serial(port, bps, timeout=timeout)
        return uart
    except Exception as result:
        print("can not open uart")
        print(result)
        return False

# 发送数据
def uart_send_data(uart, txbuf):
    len = uart.write(txbuf.encode('utf-8'))  # 写数据
    return len

# 接收数据
def uart_receive_data(uart):
    if uart.in_waiting:
        # rxdata = uart.read(uart.in_waiting).decode("utf-8")   # 以字符串接收
        rxdata = uart.read().hex()  # 以16进制(hex)接收
        # print("rxdata:",type(rxdata),rxdata)  # 打印数据
        lidarx4_handle(rxdata)


def lidarx4_handle(rx):
    # rx_buf_show.append(rx)
    if rx=='aa' and len(rx_buf)<1:
        rx_buf.append(rx)
    elif len(rx_buf)>0 and rx_buf[0]=='aa' and rx=='55':
        rx_buf.append(rx)
        if rx_buf[-2]=='aa':
            if len(rx_buf)>2:
                rx_buf_fin=rx_buf[:-2]

                try:
                    usartdata.put([rx_buf_fin,YawAngle])
                    # Lidarx4_data_process_task(rx_buf_fin)
                except:
                    print("error2")
                    pass

                rx_buf.clear()
                rx_buf.append('aa')
                rx_buf.append('55')
    elif len(rx_buf)>1 and rx_buf[0]=='aa'  and rx_buf[1]=='55':
        rx_buf.append(rx)


def Lidarx4_data_process_task(usartdata):
    global Angle_Fin
    global Distance_Fin
    global MOTORSTATE
    global LIDARSTATE
    global LidarMeasureTimes
    # print(data[2])
    while True:
        UsasrtBuffer=usartdata.get()
        Buffer=UsasrtBuffer[0]
        YawangleReal=UsasrtBuffer[1]
        # print("Data len:",Buffer)
        if MOTORSTATE==MOTORDONE and LIDARSTATE==LIDARWORKING and\
        len(Buffer)>9 and Buffer[2]=='00':
            # print("hhhhhhhhhhhhh")
            check_code = 0x55AA
            CT = int(Buffer[2],16) & 0x01
            LSN = int(Buffer[3],16)
            # print("LSC:", LSN)
            FSA = (int(Buffer[5],16) << 8 | int(Buffer[4],16))
            check_code ^= FSA

            LSA = (int(Buffer[7],16) << 8 | int(Buffer[6],16))
            CS = (int(Buffer[9],16) << 8 | int(Buffer[8],16))

            Distance = [0 for x in range(0, LSN)]
            Itensity = [0 for x in range(0, LSN)]
            if LSN==len(Buffer[10:])//3:
                for i in range(0,LSN*3,3):
                    check_code ^= int(Buffer[10 + i],16)
                    # print("i:",i,"Buffer:",int(Buffer[10 + i + 2],16),int(Buffer[10 + i + 1],16))
                    data =(int(Buffer[10 + i + 2],16) << 8 | int(Buffer[10 + i + 1],16))
                    check_code ^= data

                    Itensity[i // 3] =((int(Buffer[10 + i + 1],16) & 0x03) << 8 | int(Buffer[10 + i],16))
                    Distance[i // 3] = data >> 2
                    # Distance_Fin.append(Distance[i // 3])

                check_code ^= (LSN << 8 | CT)
                check_code ^= LSA

                Angle = [0 for x in range(0, LSN)]

                if (check_code == CS):
                    Angle_FSA = (FSA >> 1) // 64
                    Angle_LSA = (LSA >> 1) // 64
                    Angle_Diff = (Angle_LSA - Angle_FSA)
                    if (Angle_Diff < 0):
                        Angle_Diff = Angle_Diff + 360
                    for i in range(0,LSN):
                        if (LSN > 1):
                            Angle[i] = i * Angle_Diff / (LSN - 1) + Angle_FSA
                        else:
                            Angle[i] = Angle_FSA

                        if (Distance[i] > 0):
                            AngCorrect =math.atan(21.8 * (155.3 - Distance[i]) / (155.3 * Distance[i]))
                            Angle[i] = Angle[i] + AngCorrect * 180 / M_PI

                        Angle[i]=math.radians(Angle[i])
                        # Angle_Fin.append(round(Angle[i],6))
                    # plt_show(Angle, Distance)
                    # print("Distance_len:",len(Distance),"Angle_len:",len(Angle))

                    lidardata.put([Angle,Distance,YawangleReal])
                    # print(Distance_Fin,Angle_Fin)

                else:
                    print("error")
        elif len(Buffer)>9 and Buffer[2]!='00' and Buffer[3]=='01' \
            and MOTORSTATE==MOTORDONE and LIDARSTATE==LIDARWORKING:
                LidarMeasureTimes+=1
                if LidarMeasureTimes==COLLECTTIMES:
                    LidarMeasureTimes=0
                    LIDARSTATE=LIDARDONE
                    MOTORSTATE=MOTORNEEDTOWORK
                    print("measure done at:",YawangleReal)


def Lidarx4_data_show_task(lidardata):
    global YawAngle
    if SHOWREALFLAG==True:
        vis = o3d.visualization.Visualizer()
        vis.create_window(width=800, height=600)
        pointcloud = o3d.geometry.PointCloud()
        to_reset = True
        vis.add_geometry(pointcloud)

    points=[]
    while True:
        try:
            tem_data = lidardata.get()
            # tem_data = lidardata.get_nowait()
            Angle_Show = tem_data[0]
            Distance_Show = tem_data[1]
            YawangleReal_Show=tem_data[2]
            print("recoreing angle:",YawangleReal_Show)
            for i in range(len(Angle_Show)):
                if math.degrees(Angle_Show[i])>AngleLimit:
                    continue
                rec_xyz=cmath.rect(Distance_Show[i],Angle_Show[i])
                z=-rec_xyz.real
                y=rec_xyz.imag*math.cos(math.radians(YawangleReal_Show))
                x=rec_xyz.imag*math.sin(math.radians(YawangleReal_Show))
                # if abs(x)>500 or abs(y)>500 or abs(z)>500:
                #     continue
                try:
                    pointfile.write(str(x)+' '+str(y)+' '+str(z)+"\n")
                    motorfile.write(str(YawangleReal_Show)+"\n")
                    # pointfile.write(str([x,y,z])[1:-1]+"\n")
                except:
                    print("write error")
                    continue
                    #
                points.append([x,y,z])
            if SHOWREALFLAG==True:
                if SHOWSTOPFLAG==True:
                    # print("vis run")
                    vis.run()
                else:
                    pointcloud.points = o3d.utility.Vector3dVector(points)
                    vis.update_geometry(pointcloud)
                    vis.add_geometry(pointcloud)
                    if to_reset:
                        vis.reset_view_point(True)
                        to_reset = False
                    vis.poll_events()
                    vis.update_renderer()

        # except :#queue.Empty:
        except Exception as e:
            # 这个是输出错误的具体原因
            print(e)  # 输出：division by zero
            # 输出错误的具体位置，报错行号位置在第几行
            print('\n', '>>>' * 20)
            print(traceback.print_exc())
            continue


def key_task():
    global SHOWSTOPFLAG

    print("触发了")
    # print("SYSSTATE:",SYSSTATE)
    print("LIDARSTATE:", LIDARSTATE)
    print("MOTORSTATE:", MOTORSTATE)

    # SHOWSTOPFLAG=not SHOWSTOPFLAG

    pointfile.close()
    motorfile.close()
    # rawdfile.close()
    # rawdfile2.close()
    # global run_flag
    # run_flag=True
    # print("stop update")
    # Lidarx4_data_show_task.vis.run()
    # vis.run()

# 关闭串口
def close_uart(uart):
    uart.close()

def usarttask():
    global LIDARSTATE
    global MOTORSTATE
    global SYSSTATE
    # 扫描端口
    result = check_uart_port()
    if(result == False):
        return

    # 打开串口
    result = open_uart(port, bps, timeout)
    if (result == False):
        return
    else:
        uart1 = result
    while True:
        # 串口接收数据方法3
        if (LIDARSTATE==LIDARNEEDTOWORK or LIDARSTATE==LIDARWORKING) and MOTORSTATE==MOTORDONE:
            uart_receive_data(uart1)  # 接收数据
            LIDARSTATE=LIDARWORKING
        else:
            uart1.flushInput()

def tcp_serve_task():
    global SYSSTATE
    global MOTORSTATE
    global LIDARSTATE
    global YawAngle
    # TCP连接
    # 创建 socket
    tcp_server = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
    # AF_INET IPv4；AF_INET6 IPv6；SOCK_STREAM 数据流，面向连接的(TCP)； SOCK_DGRAM 数据报，无连接的(UDP)

    # 配置ip和端口
    host = socket.gethostname()  # 本地计算机名
    ip = socket.gethostbyname(host)  # 获取本地IP
    print(host, ip)
    port = 2022  # 设置可用端口
    # 绑定ip和端口
    tcp_server.bind((ip, port))  # bind函数绑定端口，有两个括号是因为参数传入的是元组，其中包含IP和端口号
    # 监听
    tcp_server.listen(2)  # 2(int)参数为backlog,代表同时最多接收n个客户端的连接申请
    #  accept函数等待连接，若连接成功返回conn和address， conn为新的套接字用于连接后的消息传输，address 连接上的客户端的地址
    conn, addr = tcp_server.accept()
    print(addr, "连接上了")
    # 单次接收处理
    data = conn.recv(1024)  # 接收数据，1024 -- bufsize参数为接收的最大数据量
    print(data.decode())  # 以字符串编码解析
    # decode函数 解码：将接收到的bytes类型二进制数据转换为str类型
    conn.send("hello client! This is server.".encode())  # 发送数据给客户端
    # encode函数 编码：将str类型转为bytes类型二进制数据去传输

    print("Tcp connect success")#tcp通讯成功才会运行到这，否则是阻塞

    #初次状态，激光雷达应该直接测量，不需要等电机到位
    if YawAngle==0:
        # print("in")
        MOTORSTATE=MOTORDONE
        LIDARSTATE=LIDARNEEDTOWORK

    conn.setblocking(0)  # 非阻塞模式
    while True:
        if MOTORSTATE==MOTORNEEDTOWORK:
            YawAngle+=1
            if YawAngle>=YAWANGLEMAX+1:
                pointfile.close()
                motorfile.close()
                YawAngle=YAWANGLEMAX
                sys.exit()
            conn.send(str(YawAngle).encode())
            MOTORSTATE=MOTORWOKING
            # print("YawAngle:",YawAngle)

        try:
            recdata = conn.recv(1024)
            if not recdata:
                print("消息已读完")
                break
            # print("recdata:",recdata.decode())
            if MOTORSTATE==MOTORWOKING and recdata.decode()=="done" :
                MOTORSTATE=MOTORDONE
                print("start to measr at:",YawAngle)
                LIDARSTATE = LIDARNEEDTOWORK

        except:
            pass
                # conn.send(str(YawAngle).encode())
                # NextFlag=False
            # print(str(y))

    tcp_server.close()  # 关闭套接字



if __name__ == '__main__':
    print("init")
    pointfile = open("data/lab3.txt", 'w')
    motorfile = open("./data/motorangle_lab3.txt", 'w')
    # rawdfile=open("./data/rawdata.txt", 'w')
    # rawdfile2 = open("./data/rawangle.txt", 'w')
    keyboard.add_hotkey('q', key_task)

    usartdata = queue.Queue()
    lidardata = queue.Queue()

    thread_usart=threading.Thread(target=usarttask)
    thread_lidardataprocess=threading.Thread(target=Lidarx4_data_process_task,args=(usartdata,))
    #是这个任务导致显示不全的
    thread_lidardatashow=threading.Thread(target=Lidarx4_data_show_task,args=(lidardata,))
    thread_tcp=threading.Thread(target=tcp_serve_task)

    thread_usart.start()
    thread_lidardataprocess.start()
    thread_lidardatashow.start()
    thread_tcp.start()
